#include "ObjectiveFunctionCollection.h"
#include "DecoArticulatedBody.h"
#include "DecoLogger.h"
#include "ConfigManager.h"



double GoStraightWithEnergyConstraints(DecoArticulatedObject* articulatedObj, double* resultDetails)
{
	std::vector<double> q;
	articulatedObj->GetQ(q);
	vector3 energy = articulatedObj->GetCumulativeEnergyConsumption();
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();

	const double minDist = 1.0;
	double deviation = abs(q[1]) + 3.5 * abs(q[2]) + abs(q[3]) + 3.5 * abs(q[4]) + abs(q[5]);
	double collision = articulatedObj->GetCumulativeCollisionVolume();
	double result = 0;

	if (energy.x >= info.mEnergyBound)
		result = energy.x;
	else if(!info.mbEvalSecondCycle)
		result = q[0] + 0.2 * deviation + 500 * collision;
	else
	{
		double distTraveledInDir = -DotProduct(vector3(q[0], q[1], q[2]) - articulatedObj->mPosFirstCycle, articulatedObj->mDirFirstCycle);
		deviation = abs(q[3] - articulatedObj->mOriFirstCycle.x) + 2.5 * abs(q[4] - articulatedObj->mOriFirstCycle.y) + abs(q[5] - articulatedObj->mOriFirstCycle.z);

		result = distTraveledInDir + 0.33 * deviation + 500 * collision;
	}

	if (resultDetails)
	{
		resultDetails[0] = result;
		for (int i = 0; i < 6; ++i)
			resultDetails[i + 1] = q[i];
		resultDetails[7] = energy.x;
		resultDetails[8] = energy.y;
		resultDetails[9] = energy.z;
	}
	return result;

}

double TurnCircularWithEnergyConstraints(DecoArticulatedObject* articulatedObj, double* resultDetails)
{

	std::vector<double> q;
	articulatedObj->GetQ(q);
	double energy = articulatedObj->GetCumulativeEnergyConsumption().x;
	double deviation = articulatedObj->GetCumulativeDeviation();
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();

	double result = 0;

	if (energy >= info.mEnergyBound)
		result = energy;
	else
		result = deviation;    
	if (resultDetails)
	{
		resultDetails[0] = result;
		for (int i = 0; i < 6; ++i)
			resultDetails[i + 1] = q[i];
		resultDetails[7] = energy;
	}
	return result;

}

double TurnRightWithEnergyConstraints(DecoArticulatedObject* articulatedObj, double* resultDetails)
{
	std::vector<double> q;
	articulatedObj->GetQ(q);
	double energy = articulatedObj->GetCumulativeEnergyConsumption().x;
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
	double result = 0;

	if (energy >= info.mEnergyBound)
		result = energy;
	else
		result = q[0] - q[1] + q[5];    
	if (resultDetails)
	{
		resultDetails[0] = result;
		for (int i = 0; i < 6; ++i)
			resultDetails[i + 1] = q[i];
		resultDetails[7] = energy;
	}
	return result;
}

double GoStraightAllowRollWithEnergyConstraints(DecoArticulatedObject* articulatedObj, double* resultDetails)
{
	std::vector<double> q;
	articulatedObj->GetQ(q);
	double energy = articulatedObj->GetCumulativeEnergyConsumption().x;
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();

	const double minDist = 1.0;
	double deviation = abs(q[1]) + abs(q[2]) + abs(q[4]) + abs(q[5]);
	double collision = articulatedObj->GetCumulativeCollisionVolume();
	double result = 0;

	if (energy >= info.mEnergyBound)
		result = energy;
	else
		result = q[0] + 0.25 * deviation + 500 * collision;    
	if (resultDetails)
	{
		resultDetails[0] = result;
		for (int i = 0; i < 6; ++i)
			resultDetails[i + 1] = q[i];
		resultDetails[7] = energy;
		resultDetails[8] = collision;
	}
	return result;
	//     char logStr[512];
	//     memset(logStr, 0, 512 * sizeof(char));
	//     sprintf(logStr, "Current Q: (%f, %f, %f, %f, %f, %f) and energy consumption %f\n", q[0], q[1], q[2], q[3], q[4], q[5], energy);
	//     cmaes_WriteLogFile(logStr);

	//if (q[0] + abs(q[1]) < -minDist) //need modification tanjie
	//	return (q[0] + deviation) / energy;
	//else
	//	return abs(q[0] + deviation + minDist);

}

double PitchUpWithEnergyConstraints(DecoArticulatedObject* articulatedObj, double* resultDetails)
{
	std::vector<double> q;
	articulatedObj->GetQ(q);
	double energy = articulatedObj->GetCumulativeEnergyConsumption().x;
	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
	double result = 0;
	double deviation = abs(q[1]) + abs(q[3]) + abs(q[5]);
	if (energy >= info.mEnergyBound)
		result = energy;
	else
		result = q[0] + q[2] + 1.5 * q[4] + 0.3 * deviation;    
	if (resultDetails)
	{
		resultDetails[0] = result;
		for (int i = 0; i < 6; ++i)
			resultDetails[i + 1] = q[i];
		resultDetails[7] = energy;
	}
	return result;
}
